#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/gicp.h>
#include <pcl/registration/transforms.h>
#include <geometry_msgs/PoseStamped.h>

class LaserOdom {
public:
    LaserOdom() {
        // 初始化ROS节点
        ros::NodeHandle nh;

        // 初始化订阅和发布的话题
        laser_sub_ = nh.subscribe("/laser", 10, &LaserOdom::laserCallback, this);
        pose_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/pose", 10);

        // 初始化点云匹配算法
        icp_.setTransformationEpsilon(1e-6);
        icp_.setEuclideanFitnessEpsilon(1e-6);
        icp_.setMaxCorrespondenceDistance(1.0);
        icp_.setUseReciprocalCorrespondences(true);
        icp_.setRANSACOutlierRejectionThreshold(1.0);

        // 初始化点云降采样器
        voxel_filter_.setLeafSize(0.1, 0.1, 0.1);
    }

private:
    ros::Subscriber laser_sub_;
    ros::Publisher pose_pub_;
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp_;
    pcl::VoxelGrid<pcl::PointXYZ> voxel_filter_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr prev_cloud_;
    Eigen::Matrix4f pose_;

    void laserCallback(const sensor_msgs::PointCloud2::ConstPtr &msg) {
        // 将ROS点云格式转换为PCL点云格式
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(*msg, *cloud);

        // 对点云进行降采样
        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        voxel_filter_.setInputCloud(cloud);
        voxel_filter_.filter(*filtered_cloud);

        if (prev_cloud_) {
            // 如果已有前一帧点云，则进行点云匹配
            icp_.setInputSource(filtered_cloud);
            icp_.setInputTarget(prev_cloud_);
            pcl::PointCloud<pcl::PointXYZ> aligned_cloud;
            icp_.align(aligned_cloud);

            // 更新机器人位姿
            pose_ = icp_.getFinalTransformation() * pose_;
        } else {
            // 如果没有前一帧点云，则将机器人初始位姿设为单位矩阵
            pose_ = Eigen::Matrix4f::Identity();
        }

        // 发布机器人位姿
        geometry_msgs::PoseStamped pose_msg;
        pose_msg.header.stamp = msg->header.stamp;
        pose_msg.header.frame_id = "map";
        pose_msg.pose.position.x = pose_(0, 3);
        pose_msg.pose.position.y = pose_(1, 3);
        pose_msg.pose.position.z = pose_(2, 3);
        Eigen::Quaternionf q(pose_.block<3, 3>(0, 0));
        pose_msg.pose.orientation.x = q.x();
        pose_msg.pose.orientation.y = q.y();
        pose_msg.pose.orientation.z = q.z();
        pose_msg.pose.orientation.w = q.w();
        pose_pub_.publish(pose_msg);

        // 更新前一帧点云
        prev_cloud_ = filtered_cloud;
    }
};

int main(int argc, char **argv) {
    // 初始化ROS节点
    ros::init(argc, argv, "cre_laser_odom");

    // 创建LaserOdom对象
    LaserOdom odom;

    // 进入ROS循环
    ros::spin();

    return 0;
}
